
#ifndef WIN32
#include <kos.h>
#define O_BINARY 0
#endif

#ifdef WIN32
#include "../koswin/koswin.h"
#endif


#ifndef WIN32
#define DBGOUT(string) printf(string);
#define DBGOUTP(string,x) printf(string,x);
#endif

#include <math.h>
#include "pgm.h"


#define DEGTORAD 0.0174532f
#define RADTODEG 57.2957795f

#define DOT(V1,V2)   ((V1).x * (V2).x + (V1).y * (V2).y + (V1).z * (V2).z)

#define MAG(V1)   sqrtf((V1).x*(V1).x + (V1).y*(V1).y + (V1).z*(V1).z)


#define SETVECV(dest,old)   \
              (dest).x=(old).x; \
              (dest).y=(old).y; \
              (dest).z=(old).z;

#define SETVEC(dest,X,Y,Z)   \
              (dest).x=X; \
              (dest).y=Y; \
              (dest).z=Z;

#define FLOAT1CLIP(amt) ((amt<1.00091f && amt> 0.9999f) ? 1 : amt)

void QuaternionFromRotation( quat* q, vector* v, float fTheta )
{
    q->x = sinf( fTheta/2.0f ) * v->x;
    q->y = sinf( fTheta/2.0f ) * v->y;
    q->z = sinf( fTheta/2.0f ) * v->z;
    q->w = cosf( fTheta/2.0f );
}

void RotationFromQuaternion( vector* v, float* fTheta, quat* q)
                                      
{
    *fTheta = acosf(q->w) * 2.0f;
    v->x    = q->x / sinf( *fTheta/2.0f );
    v->y    = q->y / sinf( *fTheta/2.0f );
    v->z    = q->z / sinf( *fTheta/2.0f );
}

void QuaternionMultiply( quat* R,quat* A, quat* B)
{
    float Dx =  A->x*B->w + A->y*B->z - A->z*B->y + A->w*B->x;
    float Dy = -A->x*B->z + A->y*B->w + A->z*B->x + A->w*B->y;
    float Dz =  A->x*B->y - A->y*B->x + A->z*B->w + A->w*B->z;
    float Dw = -A->x*B->x - A->y*B->y - A->z*B->z + A->w*B->w;

    R->x = Dx; R->y = Dy; R->z = Dz; R->w = Dw;
}

void QuaternionSlerp(quat* R,quat* A, quat* B, float fAlpha )
{
    // Compute dot product (equal to cosine of the angle between quaternions)
    float fCosTheta = A->x*B->x + A->y*B->y + A->z*B->z + A->w*B->w;
	float Bx,By,Bz,Bw;
    // Check angle to see if quaternions are in opposite hemispheres
    if( fCosTheta < 0.0f ) 
    {
        // If so, flip one of the quaterions
        fCosTheta = -fCosTheta;
        Bx = -B->x; By = -B->y; Bz = -B->z; Bw = -B->w;
    }else{
       Bx = B->x; By = B->y; Bz = B->z; Bw = B->w;
 	}

    // Set factors to do linear interpolation, as a special case where the
    // quaternions are close together.
    float fBeta = 1.0f - fAlpha;
    
    // If the quaternions aren't close, proceed with spherical interpolation
    if( 1.0f - fCosTheta > 0.001f ) 
    {   
        float fTheta = acosf( fCosTheta );
        
        fBeta  = sinf( fTheta*fBeta ) / sinf( fTheta);
        fAlpha = sinf( fTheta*fAlpha ) / sinf( fTheta);
    }

    // Do the interpolation
    R->x = fBeta*A->x + fAlpha*Bx;
    R->y = fBeta*A->y + fAlpha*By;
    R->z = fBeta*A->z + fAlpha*Bz;
    R->w = fBeta*A->w + fAlpha*Bw;
}

int load_pgm(char *filename, pgmmodel *obj)
{
	uint32 file;
	unsigned int cnt,i,objcnt;
	pgmXYZ tempXYZ;
	pgmPath tempPath;
	pgmHeader pgm_header;
	pgmmodel *currobj=obj;

	i=0;
//	DBGOUTP("loading pgm %s\r\n",filename);

	file = fs_open(filename, O_RDONLY|O_BINARY);

	if (!file) {
		DBGOUTP("Error opening %s \r\n",filename);
		return 1;
	}
	
	// load header
	cnt = fs_read(file, &pgm_header, sizeof(pgmHeader));
	if (pgm_header.ident!=PGM_MAGIC) {
		DBGOUT("invalid pgm file\r\n");
		return 1;
	}

	//load xform
	for (objcnt=0;objcnt<pgm_header.numPath;objcnt++) {
		currobj->points = 0;	
		currobj->NextPath = 0;

		//read header
		cnt = fs_read(file, &tempPath, sizeof(tempPath));
		if (tempPath.magic!=PATH_MAGIC) {DBGOUT("Path unaligned\r\n"); fs_close(file); return 1;}
		
		currobj->ID = tempPath.ID;	
		currobj->numPt = tempPath.numPt;	

		currobj->points = (vector*)malloc(currobj->numPt * sizeof(vector));

		//read pnts
		for (i=0;i<currobj->numPt;i++) {
			cnt = fs_read(file, &tempXYZ, sizeof(tempXYZ));
			currobj->points[i].x=tempXYZ.x;
			currobj->points[i].y=tempXYZ.y;
			currobj->points[i].z=tempXYZ.z;
		}
		
		if (objcnt<(pgm_header.numPath-1)) {
			currobj->NextPath = malloc(sizeof(pgmmodel));
			currobj=(pgmmodel*)currobj->NextPath;
		}else{
			currobj->NextPath=0;
		}

	}
	fs_close(file);

//	DBGOUTP("loaded %s\r\n",filename);

	return 0;
}

//return 0-normal  1-delta too small 2-delta too large 3-path doesn't exist
int pgm_getindex(int index,float* delta,vector* Vec,float* angle, pgmmodel *pgm_model)
{
	int pre,post,postpost;
	float d,tempangle1,tempangle2;
	vector tempvec1,tempvec2;
	float mag1,mag2;
	quat tempquat1,tempquat2;

	pgmmodel* currobj=pgm_model;
	while (currobj!=0) {
		if (currobj->ID==index) {

			if (*delta>=1.0f) *delta=0.9999f;
			if (*delta<0.0f) *delta=0.0f;

			pre=(int)((*delta)*((float)currobj->numPt-1));
			d=FLOAT0CLIP(((*delta)*((float)currobj->numPt-1))-((float)pre));
			post=pre+1;
			postpost=pre+2;

			if (pre>((int)currobj->numPt)-2) pre=((int)currobj->numPt)-2;
			if (post>((int)currobj->numPt)-1) post=((int)currobj->numPt)-1;
			if (postpost>((int)currobj->numPt)-1) postpost=((int)currobj->numPt)-1;
			if (pre<0) pre=0;
			if (post<1) post=1;
			if (postpost<2) postpost=2;

			Vec->x=FLOAT0CLIP((1-d)*currobj->points[pre].x+(d)*currobj->points[post].x);
			Vec->y=FLOAT0CLIP((1-d)*currobj->points[pre].y+(d)*currobj->points[post].y);
			Vec->z=FLOAT0CLIP((1-d)*currobj->points[pre].z+(d)*currobj->points[post].z);

			SETVEC(tempvec1,currobj->points[post].x-currobj->points[pre].x
					,currobj->points[post].y-currobj->points[pre].y
					,0);
			SETVEC(tempvec2,0,1,0);
			mag1 = MAG(tempvec1);
			mag2 = MAG(tempvec2);
			tempangle1=acosf(DOT(tempvec1,tempvec2)/(mag1*mag2));
			if (tempvec1.x<0) tempangle1=-tempangle1;
			
			if (post==(int)currobj->numPt-1) {*angle=tempangle1; return 0;}
			
			SETVEC(tempvec1,currobj->points[postpost].x-currobj->points[post].x
					,currobj->points[postpost].y-currobj->points[post].y
					,0);
			SETVEC(tempvec2,0,1,0);
			mag1 = MAG(tempvec1);
			mag2 = MAG(tempvec2);
			tempangle2=acosf(DOT(tempvec1,tempvec2)/(mag1*mag2));
			if (tempvec1.x<0) tempangle2=-tempangle2;

			SETVEC(tempvec1,0,0,1);
			QuaternionFromRotation(&tempquat1, &tempvec1, tempangle1);
			QuaternionFromRotation(&tempquat2, &tempvec1, tempangle2);

			QuaternionSlerp(&tempquat2,&tempquat1, &tempquat2, d);

			RotationFromQuaternion(&tempvec1, angle, &tempquat2);
			if (tempvec1.z<0) *angle=-*angle;

			return 0;
		}
		currobj=(pgmmodel*)currobj->NextPath;
	}
	Vec->x=0;
	Vec->y=0;
	Vec->z=0;
	*angle=0;

	return 3;
}



void free_pgm(pgmmodel *pgm_model) 
{
	pgmmodel* currobj=pgm_model;
	pgmmodel* oldobj;

	if (currobj->points) free(currobj->points);
	currobj->points=NULL;
	currobj->numPt=0;

	while (currobj!=0) {
		if (currobj->points) free(currobj->points);
		oldobj=currobj;
		currobj=(pgmmodel*)currobj->NextPath;
		if (oldobj!=pgm_model) {if (oldobj) free(oldobj);}
	}
}
